#include <string>
#include "octree.h"
#include "LASlib/lasreader.hpp"
#include "LASlib/laswriter.hpp"
#include <osgViewer/Viewer>
#include <osg/Geometry>
#include <osg/Drawable>
#include <iostream>
#include <fstream>

std::shared_ptr<octree> importLas(const std::string& filename)
{
	LASreadOpener lasreadopener;
	lasreadopener.set_file_name(filename.c_str());
	LASreader* lasreader = lasreadopener.open();
	
	unsigned pointsize = lasreader->npoints;
	double minx = lasreader->header.min_x;
	double miny = lasreader->header.min_y;
	double minz = lasreader->header.min_z;
	double maxx = lasreader->header.max_x;
	double maxy = lasreader->header.max_y;
	double maxz = lasreader->header.max_z;

	double lenx = fabs(maxx - minx);
	double leny = fabs(maxy - miny);
	double lenz = fabs(maxz - minz);
	double volume = lenx;
	if (leny > volume) volume = leny;
	if (lenz > volume) volume = lenz;

	std::shared_ptr<octree> otree = std::make_shared<octree>(
		minx, miny, minz, volume);
	otree->SetResolution(0.2);

	unsigned i = 0;
	while (lasreader->read_point())
	{
		las_Point lasPoint;
		lasPoint.point_x = lasreader->point.get_x();
		lasPoint.point_y = lasreader->point.get_y();
		lasPoint.point_z = lasreader->point.get_z();
		lasPoint.label = lasreader->point.get_classification();
		otree->AddPoint(lasPoint);
		i++;
	}
	lasreader->close();
	delete lasreader;
	return otree;
}

osg::Node* createPoint(std::vector<Eigen::Vector3d> points)
{
	osg::Vec3dArray* v3ds = new osg::Vec3dArray;
	for (Eigen::Vector3d las_pt : points)
	{
		v3ds->push_back(osg::Vec3d(las_pt.x(),
			las_pt.y(), las_pt.z()));
	}

	osg::Geometry* geom = new osg::Geometry;
	geom->setVertexArray(v3ds);
	geom->addPrimitiveSet(new osg::DrawArrays(GL_POINTS, 0, v3ds->size()));

	osg::Geode* geode = new osg::Geode;
	geode->addDrawable(geom);
	return geode;
}



int main(int argc, char** argv)
{
	if (argc != 2)
	{
		return -1;
	}

	char* filepath = argv[1];
	std::shared_ptr<octree> otree = importLas(filepath);
	otree->Simplify();
	std::cout << filepath << " simplification is completed! " << std::endl;
	getchar();
	std::vector<Eigen::Vector3d> total_points = otree->GetPoints();

	std::ofstream ofs("cloud.txt");
	ofs.precision(10);
	for (Eigen::Vector3d pt : total_points)
	{
		ofs << pt.x() << " " << pt.y() << " " << pt.z() << std::endl;
	}
	ofs.close();
	/*osg::ref_ptr<osgViewer::Viewer> viewer = new osgViewer::Viewer;
	osg::ref_ptr<osg::Group> root = new osg::Group;
	root->addChild(createPoint(total_points));
	viewer->setSceneData(root);
	return viewer->run();*/
}